#include <stdio.h>
#include <avr/pgmspace.h>
#include "can_avr.h"
#include "em36.h"
#include "lcd.h"

#define VSS_TEETH		8
#define VSS_MAX_STEP 	16

#define TIMEOUT	(5000/100)				// 5000ms at 100ms rate

static uint16_t _timeout;

uint16_t EM36_engineSpeed;
uint8_t EM36_vehicleSpeed;
uint8_t EM36_fuelP;			// bar *10
uint8_t EM36_oilP;			// bar *10
uint8_t EM36_flags;
uint8_t EM36_throttle;		// 0:255 = 0:99.6%
uint8_t EM36_battery;		// battery V * 16 (0 to 15.93volts)
uint8_t	EM36_coolant;
uint8_t EM36_CAN_count;
uint16_t EM36_lambda;		// lambda * 1000
uint8_t EM36_AIT;			// should go negative but the sensor hasn't been cal'd and we've got no signed display :-)


mobCtrl_t rxMsg301;
mobCtrl_t rxMsg302;

extern bool bTestMode;


void EM36_init( void )
{
	CAN_objRx( &rxMsg301, 0, 0x301, 8 );
	CAN_objRx( &rxMsg302, 1, 0x302, 8 );
	_timeout = TIMEOUT;
}


void EM36_task100ms( void )
{
	uint32_t dw;
	
	if( CAN_objStatus(&rxMsg301) == MOB_Complete ) {
		
		EM36_CAN_count++;

		EM36_vehicleSpeed = rxMsg301.data[0];

		dw = rxMsg301.data[1];
		dw = dw * 100;
		dw = dw / 256;
		EM36_oilP = dw;	//bar*10
		
		dw = (rxMsg301.data[2] << 8) + rxMsg301.data[3];
		dw = dw * 100;
		dw = dw >> 8;											// divider by 256
		EM36_engineSpeed = (dw & 0xffff);
		
		EM36_throttle = rxMsg301.data[4];
		dw = rxMsg301.data[5];
		dw = dw * 10;
		dw = dw / 16;
		EM36_battery = dw;										// battery in 100mV units
		//Fuel total = data[6:7] * 2

		_timeout = TIMEOUT;
	}
	
	if( CAN_objStatus(&rxMsg302) == MOB_Complete ) {
		
		EM36_CAN_count++;

		//EM36_T4Count = (rxMsg302.data[0] << 8) + rxMsg302.data[1];
		dw = rxMsg302.data[2];
		dw = dw * 250;
		dw = dw / 32;
		EM36_lambda = dw;	/* lambda stored as lambda * 1000 - ie 1000 = stoich */

		dw = rxMsg302.data[3];
		dw = (dw * 100);
		dw = dw / 256;
		if( dw > 50 ) {
			EM36_AIT = dw - 50;
		}
		else {
			EM36_AIT = 0;
		}
		if( rxMsg302.data[4] >= 40 ) {
			EM36_coolant = rxMsg302.data[4] - 40;							//(-40 to 215)
		}
		else {
			EM36_coolant = 0;
		}
		EM36_flags = rxMsg302.data[5];

		dw = rxMsg302.data[1];
		dw = dw * 100;
		dw = dw / 256;
		EM36_fuelP = dw;	//bar*10

		_timeout = TIMEOUT;
	}

	if( _timeout ) {
		_timeout--;
	}
	else {
		if( bTestMode ) {
			EM36_engineSpeed += 10;
			if( EM36_engineSpeed >= 10000 ) EM36_engineSpeed = 0;

			EM36_battery++;
			EM36_coolant++;
			EM36_oilP++;
			EM36_vehicleSpeed++;
			EM36_throttle++;
			EM36_flags++;
			EM36_fuelP++;
			EM36_lambda++;
			EM36_AIT++;
		}
		else {
			EM36_engineSpeed = 0;
			EM36_battery = 0;
			EM36_coolant = 0;
			EM36_oilP = 0;
			EM36_vehicleSpeed = 0;
			EM36_throttle = 0;
			EM36_flags = 0;
		}
	}

}
